/*
 * Ken Anderson, 2009
 */

#include "Heuristic.h"
#include "math.h"
#include <queue>
#include <vector>
#include "util.h"

static const double INITIAL_MAX_HEURISTIC = -1.0;	// For coloring purposes.

Heuristic::Heuristic( )
{
	mGraph = NULL;
	mGoal = NULL;
	mMaxValue = INITIAL_MAX_HEURISTIC;
	mNumExpanded = 0;
}

Heuristic::Heuristic( Graph * graph, const std::string name ) : mName(name)
{
	mGraph = graph;
	mGoal = NULL;
	mMaxValue = INITIAL_MAX_HEURISTIC;
	mNumExpanded = 0;
}

Heuristic::~Heuristic()
{
}

void Heuristic::Init( Node * start, Node * goal )
{
	mGoal = goal;
	mMaxValue = INITIAL_MAX_HEURISTIC;
	mNumExpanded = 0;
}

// Could be faster.  This uses a linear scan.
float Heuristic::GetMaxValue()
{
	if( mMaxValue == INITIAL_MAX_HEURISTIC )
	{
		for( unsigned int i=0; i<mGraph->GetNumOfNodes(); i++)
		{
			float hValue = ShowValue(mGraph->GetNode(i));
			if( hValue > mMaxValue )
				mMaxValue = hValue;
		}
	}
	return mMaxValue;
}

void Heuristic::print( FILE * stream )
{
	if(TRACE) printf("Heuristic Values (maxValue=%f) \n", GetMaxValue() );
	for( unsigned int i=0; i<mGraph->GetNumOfNodes(); i++ )
	{
		Node * node = mGraph->GetNode(i);
		if(TRACE) printf("ID=%i h=%f \n", node->GetID(), GetValue( node ) );
	}
	if(TRACE) printf("\n");
}

ZeroHeuristic::ZeroHeuristic( Graph * graph) : Heuristic( graph, "No Heuristic" )
{
}

ManhattanDistanceHeuristic::ManhattanDistanceHeuristic( Graph * graph ) : Heuristic( graph, "ManhattanDistanceHeuristic" )
{
}

float ManhattanDistanceHeuristic::GetValue( Node * node )
{
	return fabs( node->GetX() - mGoal->GetX() ) + fabs( node->GetY() - mGoal->GetY() );
}

XDistanceHeuristic::XDistanceHeuristic( Graph * graph ) : Heuristic( graph, "XDistanceHeuristic" )
{
}

float XDistanceHeuristic::GetValue( Node * node )
{
	return fabs( node->GetX() - mGoal->GetX() );
}

YDistanceHeuristic::YDistanceHeuristic( Graph * graph ) : Heuristic( graph, "YDistanceHeuristic" )
{
}

float YDistanceHeuristic::GetValue( Node * node )
{
	return fabs( node->GetY() - mGoal->GetY() );
}

ResumablePerfectHeuristic::ResumablePerfectHeuristic( Graph * graph, bool onDemand, bool useHeuristic ) 
	: ResumableHeuristic( graph, "ResumablePerfectHeuristic", onDemand )
{
	if( useHeuristic ) {
		mBackwardsHeuristic = new ManhattanDistanceHeuristic( mGraph );	// Fast
	} else {
		mBackwardsHeuristic = new ZeroHeuristic( mGraph );	// Slow
	}
	mBackwardsSearch = new AStar( mGraph, mBackwardsHeuristic );
}

ResumablePerfectHeuristic::~ResumablePerfectHeuristic()
{
	delete mBackwardsSearch;
	delete mBackwardsHeuristic;
}

void ResumablePerfectHeuristic::Init( Node * start, Node * goal )
{
	mGoal = goal;
	mMaxValue = INITIAL_MAX_HEURISTIC;
	mNumExpanded = 0;
	
	mBackwardsSearch->Init(goal,start);		// reverse start and goal
}

float ResumablePerfectHeuristic::GetValue( Node * node )
{
	float minCost = mBackwardsSearch->ResumeSearch(node, mOnDemand);
	mNumExpanded = mBackwardsSearch->NumExpandedNodes();
	return minCost;
}

float ResumablePerfectHeuristic::ShowValue( Node * node )
{
	if( mBackwardsSearch->IsClosed(node) )
		return mBackwardsSearch->GetGVal(node);
	return 0.0;
}

unsigned int ResumablePerfectHeuristic::GetMaxOpenListSize() 
{
	return mBackwardsSearch->GetMaxOpenListSize(); 
}

ResumableYHeuristic::ResumableYHeuristic( Graph * graph, bool onDemand, bool useHeuristic )
	: ResumableHeuristic( graph, "ResumableYHeuristic", onDemand )
{
	InitAbstractGraph();
	if( useHeuristic ) {
		mBackwardsHeuristic = new YDistanceHeuristic( &mAbstractGraph );	// Fast
	} else {
		mBackwardsHeuristic = new ZeroHeuristic( &mAbstractGraph );	// Slow
	}	
	mBackwardsSearch = new AStar( &mAbstractGraph, mBackwardsHeuristic );
}

ResumableYHeuristic::~ResumableYHeuristic()
{
	delete mBackwardsSearch;
	delete mBackwardsHeuristic;
}

void ResumableYHeuristic::InitAbstractGraph()
{
	Node * subnode = NULL;
	int supernodeID = 0;
	// Index to supernode.  
	// This is required because we cannot add the actual edges until the graph is fully created!!
	NodeValueMap_t supernodeIDs;	

	// Create abstract/super nodes 
	// ** CANNOT ADD EDGES UNTIL GRAPH FULLY CREATED!! **
	for( unsigned int i=0; i<mGraph->GetNumOfNodes(); i++)
	{
		subnode = mGraph->GetNode(i);

		// create new supernode
		if( supernodeIDs.find(subnode)==supernodeIDs.end() )
		{
			supernodeID = mAbstractGraph.AddNode(subnode->GetX(),subnode->GetY());
			// check all successors and link to corresponding supernode if necessary
			MarkNodeAndSuccessors( subnode, supernodeID, supernodeIDs );
		}
	}

	// Connect subnode(s) to supernode(s).  
	// Doesn't matter if the supernode hasn't changed
	// CANNOT ADD EDGES UNTIL GRAPH FULLY CREATED, that is why we have a separate loop
	for( unsigned int i=0; i<mGraph->GetNumOfNodes(); i++)
	{
		subnode = mGraph->GetNode(i);
		mSupernodes[ subnode ] = mAbstractGraph.GetNode( supernodeIDs[ subnode ] );
		if(TRACE) printf(" added link from subnodeID=%i to superNodeID=%i \n", 
			subnode->GetID(), mSupernodes[ subnode ]->GetID() );	
	}

	// Connect supernodes with edges 
	// (an abstract edge's weight is the minimal weight of the set of edges ...
	for( unsigned int i=0; i<mGraph->GetNumOfNodes(); i++)
	{
		Node * node = mGraph->GetNode(i);
		for( unsigned int j=0; j<node->NumOutEdges(); j++ )
		{
			Edge * edge = node->GetOutEdge(j);
			Edge * absEdge = mAbstractGraph.GetEdge( mSupernodes[node], mSupernodes[edge->ToNode()] );
			// Edge exists already, update the weight
			if( absEdge && absEdge->GetWeight() < edge->GetWeight() )
				absEdge->SetWeight( edge->GetWeight() );
			// Edge doesn't exist - create it
			else if( !absEdge )
				mAbstractGraph.AddEdge(mSupernodes[node], mSupernodes[edge->ToNode()], edge->GetWeight());
		}
	}

	// Print for error checking
	if(TRACE) printf(" The abstract graph \n");
	mAbstractGraph.print(stdout);
}

// Recursive function - checks all successor nodes.  If Part of abstract node, links them to the abstract node.
void ResumableYHeuristic::MarkNodeAndSuccessors( Node * subnode, int supernodeID, NodeValueMap_t & supernodeIDs )
{
	// Ending condition - already marked or on different row
	if( supernodeIDs.find(subnode)!=supernodeIDs.end() || 
		!aboutEqualTo(subnode->GetY(), mAbstractGraph.GetNode((unsigned int)(supernodeID))->GetY()) )
		return;

	// Continue and expand
	supernodeIDs[subnode] = supernodeID;
	for( unsigned int i=0; i<subnode->NumOutEdges(); i++)
	{
		MarkNodeAndSuccessors( subnode->GetOutEdge(i)->ToNode(), supernodeID, supernodeIDs );
	}
}

void ResumableYHeuristic::Init( Node * start, Node * goal )
{
	mGoal = goal;
	mMaxValue = INITIAL_MAX_HEURISTIC;
	mNumExpanded = 0;
	
	mBackwardsSearch->Init( mSupernodes[goal], mSupernodes[start] );		// reverse start and goal
}

float ResumableYHeuristic::GetValue( Node * node )
{
	float minCost = mBackwardsSearch->ResumeSearch( mSupernodes[node], mOnDemand );
	mNumExpanded = mBackwardsSearch->NumExpandedNodes();
	return minCost;
}

float ResumableYHeuristic::ShowValue( Node * node )
{
	if( mBackwardsSearch->IsClosed( mSupernodes[node] ) )
		return mBackwardsSearch->GetGVal( mSupernodes[node] );
	return 0.0;
}

unsigned int ResumableYHeuristic::GetMaxOpenListSize() 
{
	return mBackwardsSearch->GetMaxOpenListSize(); 
}


ResumableXHeuristic::ResumableXHeuristic( Graph * graph, bool onDemand, bool useHeuristic ) 
	: ResumableHeuristic( graph, "ResumableXHeuristic", onDemand )
{
	InitAbstractGraph();
	if( useHeuristic ) {
		mBackwardsHeuristic = new XDistanceHeuristic( &mAbstractGraph );	// Fast
	} else {
		mBackwardsHeuristic = new ZeroHeuristic( &mAbstractGraph );	// Slow
	}
	mBackwardsSearch = new AStar( &mAbstractGraph, mBackwardsHeuristic );
}

ResumableXHeuristic::~ResumableXHeuristic()
{
	delete mBackwardsSearch;
	delete mBackwardsHeuristic;
}

void ResumableXHeuristic::InitAbstractGraph()
{
	Node * subnode = NULL;
	int supernodeID = 0;
	// Index to supernode.  
	// This is required because we cannot add the actual edges until the graph is fully created!!
	NodeValueMap_t supernodeIDs;

	// Create abstract/super nodes 
	// ** CANNOT ADD EDGES UNTIL GRAPH FULLY CREATED!! **
	for( unsigned int i=0; i<mGraph->GetNumOfNodes(); i++)
	{
		subnode = mGraph->GetNode(i);

		// create new supernode
		if( supernodeIDs.find(subnode)==supernodeIDs.end() )
		{
			supernodeID = mAbstractGraph.AddNode(subnode->GetX(),subnode->GetY());
			// check all successors and link to corresponding supernode if necessary
			MarkNodeAndSuccessors( subnode, supernodeID, supernodeIDs );
		}
	}

	// Connect subnode(s) to supernode(s).  
	// Doesn't matter if the supernode hasn't changed
	// CANNOT ADD EDGES UNTIL GRAPH FULLY CREATED, that is why we have a separate loop
	for( unsigned int i=0; i<mGraph->GetNumOfNodes(); i++)
	{
		subnode = mGraph->GetNode(i);
		mSupernodes[ subnode ] = mAbstractGraph.GetNode( supernodeIDs[ subnode ] );
		if(TRACE) printf(" added link from subnodeID=%i to superNodeID=%i \n", 
			subnode->GetID(), mSupernodes[ subnode ]->GetID() );	
	}

	// Connect supernodes with edges 
	// (an abstract edge's weight is the minimal weight of the set of edges ...
	for( unsigned int i=0; i<mGraph->GetNumOfNodes(); i++)
	{
		Node * node = mGraph->GetNode(i);
		for( unsigned int j=0; j<node->NumOutEdges(); j++ )
		{
			Edge * edge = node->GetOutEdge(j);
			Edge * absEdge = mAbstractGraph.GetEdge( mSupernodes[node], mSupernodes[edge->ToNode()] );
			// Edge exists already, update the weight
			if( absEdge && absEdge->GetWeight() < edge->GetWeight() )
				absEdge->SetWeight( edge->GetWeight() );
			// Edge doesn't exist - create it
			else if( !absEdge )
				mAbstractGraph.AddEdge(mSupernodes[node], mSupernodes[edge->ToNode()], edge->GetWeight());
		}
	}

	// Print for error checking
	if(TRACE) printf(" The abstract graph \n");
	mAbstractGraph.print(stdout);
}

// Recursive function - checks all successor nodes.  If Part of abstract node, links them to the abstract node.
void ResumableXHeuristic::MarkNodeAndSuccessors( Node * subnode, int supernodeID, NodeValueMap_t & supernodeIDs )
{
	// Ending condition - already marked or on different row
	if( supernodeIDs.find(subnode)!=supernodeIDs.end() || 
		!aboutEqualTo(subnode->GetX(), mAbstractGraph.GetNode((unsigned int)(supernodeID))->GetX()) )
		return;

	// Continue and expand
	supernodeIDs[subnode] = supernodeID;
	for( unsigned int i=0; i<subnode->NumOutEdges(); i++)
	{
		MarkNodeAndSuccessors( subnode->GetOutEdge(i)->ToNode(), supernodeID, supernodeIDs );
	}
}

void ResumableXHeuristic::Init( Node * start, Node * goal )
{
	mGoal = goal;
	mMaxValue = INITIAL_MAX_HEURISTIC;
	mNumExpanded = 0;
	
	mBackwardsSearch->Init( mSupernodes[goal], mSupernodes[start] );		// reverse start and goal
}

float ResumableXHeuristic::GetValue( Node * node )
{
	float minCost = mBackwardsSearch->ResumeSearch( mSupernodes[node], mOnDemand );
	mNumExpanded = mBackwardsSearch->NumExpandedNodes();
	return minCost;
}

float ResumableXHeuristic::ShowValue( Node * node )
{
	if( mBackwardsSearch->IsClosed( mSupernodes[node] ) )
		return mBackwardsSearch->GetGVal( mSupernodes[node] );
	return 0.0;
}

unsigned int ResumableXHeuristic::GetMaxOpenListSize() 
{
	return mBackwardsSearch->GetMaxOpenListSize(); 
}

ResumableAdditiveHeuristic::ResumableAdditiveHeuristic( Graph * graph, bool onDemand, bool useHeuristic ) 
	: Heuristic( graph, "ResumableAdditiveHeuristic" )
{
	mXHeuristic = new ResumableXHeuristic( graph, onDemand, useHeuristic );
	mYHeuristic = new ResumableYHeuristic( graph, onDemand, useHeuristic );
}

ResumableAdditiveHeuristic::~ResumableAdditiveHeuristic()
{
	if(mXHeuristic)
		delete mXHeuristic;
	if(mYHeuristic)
		delete mYHeuristic;
}

// Only uses the goal
void ResumableAdditiveHeuristic::Init( Node * start, Node * goal )
{
	// set the goal
	mGoal = goal;
	mMaxValue = INITIAL_MAX_HEURISTIC;
	mNumExpanded = 0;

	// Initialize the component heuristics
	mXHeuristic->Init(start, goal);
	mYHeuristic->Init(start, goal);

	// Set number of expanded nodes
	mNumExpanded = mXHeuristic->NumExpandedNodes() + mYHeuristic->NumExpandedNodes();
}

float ResumableAdditiveHeuristic::GetValue( Node * node )
{
	float value = mXHeuristic->GetValue(node) + mYHeuristic->GetValue(node);
	mNumExpanded = mXHeuristic->NumExpandedNodes() + mYHeuristic->NumExpandedNodes();
	return value;
}

float ResumableAdditiveHeuristic::ShowValue( Node * node )
{
	return mXHeuristic->ShowValue(node) + mYHeuristic->ShowValue(node);
}

unsigned int ResumableAdditiveHeuristic::GetMaxOpenListSize() 
{
	return mYHeuristic->GetMaxOpenListSize() + mXHeuristic->GetMaxOpenListSize();
}
